﻿using System;

namespace MauiControl.Converters
{
    public class QuaternionFusionConverter
    {
        private float q0 = 1.0f; // 四元数实部
        private float q1 = 0.0f; // 四元数虚部 (i)
        private float q2 = 0.0f; // 四元数虚部 (j)
        private float q3 = 0.0f; // 四元数虚部 (k)

        // 更新时间步长
        private float deltaT = 0.01f; // 10ms

        // 重力加速度（用于姿态判断）
        private const float Gravity = 9.81f;

        // 静止状态阈值（角速度小于该值时认为设备静止）
        private const float StationaryThreshold = 0.01f;

        // 静止状态标志
        private bool _isStationary = false;

        // 当前设备姿态
        private DeviceOrientation _currentOrientation = DeviceOrientation.Default;

        /// <summary>
        /// 设备姿态枚举
        /// </summary>
        private enum DeviceOrientation
        {
            Default,    // 默认姿态（平放）
            Portrait,   // 竖放（Z 轴垂直）
            Landscape   // 侧放（Y 轴垂直）
        }

        /// <summary>
        /// 更新四元数
        /// </summary>
        /// <param name="gx">X 轴角速度</param>
        /// <param name="gy">Y 轴角速度</param>
        /// <param name="gz">Z 轴角速度</param>
        /// <param name="ax">X 轴加速度</param>
        /// <param name="ay">Y 轴加速度</param>
        /// <param name="az">Z 轴加速度</param>
        public void UpdateQuaternion(float gx, float gy, float gz, float ax, float ay, float az)
        {
            // 检测设备姿态
            _currentOrientation = DetectOrientation(ax, ay, az);

            // 根据设备姿态调整 XYZ 轴的定义
            switch (_currentOrientation)
            {
                case DeviceOrientation.Portrait:
                    // 竖放：交换 X 轴和 Z 轴
                    (gx, gz) = (gz, gx);
                    (ax, az) = (az, ax);
                    break;

                case DeviceOrientation.Landscape:
                    // 侧放：交换 Y 轴和 Z 轴
                    (gy, gz) = (gz, gy);
                    (ay, az) = (az, ay);
                    break;

                case DeviceOrientation.Default:
                default:
                    // 平放：保持默认的 XYZ 轴定义
                    break;
            }

            // 检测设备是否静止
            float angularSpeed = (float)Math.Sqrt(gx * gx + gy * gy + gz * gz);
            _isStationary = angularSpeed < StationaryThreshold;

            if (_isStationary)
            {
                Console.WriteLine("设备静止，停止更新四元数");
                return; // 设备静止时，不更新四元数
            }

            // 归一化加速度数据
            float norm = (float)Math.Sqrt(ax * ax + ay * ay + az * az);
            ax /= norm;
            ay /= norm;
            az /= norm;

            // 计算误差（加速度与重力方向的偏差）
            float errorX = ay * q3 - az * q2;
            float errorY = az * q1 - ax * q3;
            float errorZ = ax * q2 - ay * q1;

            // 修正角速度
            gx += errorX * deltaT;
            gy += errorY * deltaT;
            gz += errorZ * deltaT;

            // 四元数微分方程
            float q0Dot = (-q1 * gx - q2 * gy - q3 * gz) * 0.5f;
            float q1Dot = (q0 * gx + q2 * gz - q3 * gy) * 0.5f;
            float q2Dot = (q0 * gy - q1 * gz + q3 * gx) * 0.5f;
            float q3Dot = (q0 * gz + q1 * gy - q2 * gx) * 0.5f;

            // 更新四元数
            q0 += q0Dot * deltaT;
            q1 += q1Dot * deltaT;
            q2 += q2Dot * deltaT;
            q3 += q3Dot * deltaT;

            // 归一化四元数
            NormalizeQuaternion();
        }

        /// <summary>
        /// 检测设备姿态
        /// </summary>
        private DeviceOrientation DetectOrientation(float ax, float ay, float az)
        {
            // 判断设备姿态
            if (Math.Abs(az) > 0.9f) // Z 轴接近垂直
            {
                return DeviceOrientation.Default; // 平放
            }
            else if (Math.Abs(ax) > 0.9f) // X 轴接近垂直
            {
                return DeviceOrientation.Portrait; // 竖放
            }
            else if (Math.Abs(ay) > 0.9f) // Y 轴接近垂直
            {
                return DeviceOrientation.Landscape; // 侧放
            }
            else
            {
                return DeviceOrientation.Default; // 默认姿态
            }
        }

        /// <summary>
        /// 归一化四元数
        /// </summary>
        private void NormalizeQuaternion()
        {
            float norm = (float)Math.Sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);

            // 检查 norm 是否为零或无效
            if (norm == 0 || float.IsNaN(norm) || float.IsInfinity(norm))
            {
                Console.WriteLine("四元数归一化失败: norm 无效");
                q0 = 1.0f;
                q1 = 0.0f;
                q2 = 0.0f;
                q3 = 0.0f;
                return;
            }

            q0 /= norm;
            q1 /= norm;
            q2 /= norm;
            q3 /= norm;

            Console.WriteLine($"归一化四元数: q0={q0}, q1={q1}, q2={q2}, q3={q3}");
        }

        /// <summary>
        /// 获取四元数
        /// </summary>
        public float[] GetQuaternion()
        {
            return new float[] { q0, q1, q2, q3 };
        }

        /// <summary>
        /// 通过四元数计算欧拉角度
        /// </summary>
        public float[] CalculateEulerAngles()
        {
            double qw = q0;
            double qx = q1;
            double qy = q2;
            double qz = q3;

            // 计算 Roll (X轴旋转)
            double sinr_cosp = 2 * (qw * qx + qy * qz);
            double cosr_cosp = 1 - 2 * (qx * qx + qy * qy);
            double roll = Math.Atan2(sinr_cosp, cosr_cosp);

            // 计算 Pitch (Y轴旋转)
            double sinp = 2 * (qw * qy - qz * qx);
            double pitch;
            if (Math.Abs(sinp) >= 1)
            {
                pitch = Math.CopySign(Math.PI / 2, sinp); // 使用 90° 如果超出范围
            }
            else
            {
                pitch = Math.Asin(sinp);
            }

            // 计算 Yaw (Z轴旋转)
            double siny_cosp = 2 * (qw * qz + qx * qy);
            double cosy_cosp = 1 - 2 * (qy * qy + qz * qz);
            double yaw = Math.Atan2(siny_cosp, cosy_cosp);

            // 将弧度转换为角度
            roll = roll * (180 / Math.PI);
            pitch = pitch * (180 / Math.PI);
            yaw = yaw * (180 / Math.PI);

            Console.WriteLine($"计算欧拉角: Roll={roll}, Pitch={pitch}, Yaw={yaw}");

            return new float[] { (float)roll, (float)pitch, (float)yaw };
        }

        /// <summary>
        /// 判断芯片的当前状态
        /// </summary>
        /// <param name="ax">X 轴加速度</param>
        /// <param name="ay">Y 轴加速度</param>
        /// <param name="az">Z 轴加速度</param>
        /// <returns>芯片状态描述</returns>
        public string GetChipState(float ax, float ay, float az)
        {
            // 判断芯片的姿态
            if (Math.Abs(az) > 0.9f) // Z 轴接近垂直
            {
                return "Z 轴垂直";
            }
            else if (Math.Abs(ax) > 0.9f) // X 轴接近垂直
            {
                return "X 轴垂直";
            }
            else if (Math.Abs(ay) > 0.9f) // Y 轴接近垂直
            {
                return "Y 轴垂直";
            }
            else
            {
                return "未知状态";
            }
        }
    }
}